#include "Patroller.h"

/**
 * Constructor- creates a new object to run the patrol
 */
Patroller::Patroller(const float& waitAtGoal, const int& numLaps,
		const std::string& logfileName, const double& waitForServer) :
		m_actionClient("move_base", true) {

	// initialize members
	m_succLocs = 0;
	m_totalDist = 0.0;
	m_startTime = ros::Time::now();
	m_waitAtGoal = waitAtGoal;
	m_numLaps = numLaps;

	// open log file
	m_logFile.open(logfileName.c_str());

    // wait for the action server to become available
	ROS_INFO("Waiting for the move_base action server");
	m_actionClient.waitForServer(ros::Duration(waitForServer));
    ROS_INFO("Connected to move base server");
}

/**
 * Destructor- closes log file
 */
Patroller::~Patroller() {
	// close log file
	m_logFile.close();
}

/**
 * Runs the robot along given patrol path
 */
void Patroller::runPatrol(const std::vector<geometry_msgs::Point>& patrolPath,
		const float& initCost, std::vector<float> segmentsCost) {

	// get to patrol path's starting point
	bool success = sendGoal(patrolPath[0]);

	// update data and log result
	handleGoal(success, initCost);

	// wait in place
	ros::Duration(m_waitAtGoal).sleep();

	// set index of next patrol location
	int currPatrolLoc = 1;

	// keep running patrol until force quit
	for (int currLap = 0 ; currLap < m_numLaps ; ++currLap) {

		// inform user
		ROS_INFO_STREAM("starting round #" << currLap);

		// iterate over all patrol locations
		for ( ; currPatrolLoc < patrolPath.size() ; ++currPatrolLoc) {

			// send current goal
			ROS_INFO_STREAM("next patrol index: " << currPatrolLoc);
			bool success = sendGoal(patrolPath[currPatrolLoc]);

			// update data and log result
			handleGoal(success, segmentsCost[currPatrolLoc]);

			// wait in place
			ros::Duration(m_waitAtGoal).sleep();
		}

		// return to starting point
		currPatrolLoc = 0;

		// initialize data before next round
		m_succLocs = 0;
	}
}

/**
 * Uses actionlib client to send given goal
 */
bool Patroller::sendGoal(const geometry_msgs::Point& goalPos) {

	// initialize goal message to move base
    move_base_msgs::MoveBaseGoal goalMsg;
    goalMsg.target_pose.header.frame_id = "map";
    goalMsg.target_pose.header.stamp = ros::Time::now();

    // set goal position
    goalMsg.target_pose.pose.position.x = goalPos.x;
    goalMsg.target_pose.pose.position.y = goalPos.y;
    goalMsg.target_pose.pose.orientation = angleToQuat(goalPos.z);

    // send goal
	ROS_INFO_STREAM("Sending goal: " << goalPos.x << "," << goalPos.y);
    m_actionClient.sendGoal(goalMsg);

    // Wait for the action to return
    m_actionClient.waitForResult();

    // return indication for success
	return (m_actionClient.getState()
			== actionlib::SimpleClientGoalState::SUCCEEDED);
}

/**
 * Updates counter of successful arrivals, total distance traveled and calculates
 * the elapsed time. Writes the data to log file
 */
void Patroller::handleGoal(const bool& isSuccess, const float& segmentCost) {

	// update number of successful locations
	if (isSuccess) {
		++m_succLocs;
	}

	// update distance traveled
	m_totalDist += segmentCost;

	// get elapsed time
	ros::Duration elapsedTime = ros::Time::now() - m_startTime;
	m_startTime = ros::Time::now();

	// log current patrol location data
	writeLine(m_succLocs, m_totalDist, elapsedTime);
}

/**
 * Writes given data as a new log entry
 */
void Patroller::writeLine(const int& numSuccLoc, const float& totalDist,
		ros::Duration elapsedTime) {

	// log data
	m_logFile << "Successful locations in current round: " << numSuccLoc
			<< " total distance traveled: " << totalDist << " time elapsed: "
			<< elapsedTime << std::endl;
}

/**
 * Returns quaternion matching given angle
 */
geometry_msgs::Quaternion Patroller::angleToQuat(const float& angle) {

	// convert angle to radians
	double radians = angle * (M_PI/180);

	// return quaternion
	tf::Quaternion quaternion;
	quaternion = tf::createQuaternionFromYaw(radians);
	geometry_msgs::Quaternion qMsg;
	tf::quaternionTFToMsg(quaternion, qMsg);

	return qMsg;
}
